import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Circle 
from mpl_toolkits.mplot3d import Axes3D
import copy
from celluloid import Camera  # 保存动图时用，current_positionp install celluloid
from math import sqrt, pow, pi, cos, sin, atan2
from numpy.random import rand,randint
import time 
import elastic_bands_3D
import vtool as vt

def plot_opaque_cube(fig,ax,x=10, y=20, z=30, dx=40, dy=50, dz=60):

    xx = np.linspace(x, x+dx, 2)
    yy = np.linspace(y, y+dy, 2)
    zz = np.linspace(z, z+dz, 2)

    xx2, yy2 = np.meshgrid(xx, yy)

    ax.plot_surface(xx2, yy2, np.full_like(xx2, z))
    ax.plot_surface(xx2, yy2, np.full_like(xx2, z+dz))
   

    yy2, zz2 = np.meshgrid(yy, zz)
    ax.plot_surface(np.full_like(yy2, x), yy2, zz2)
    ax.plot_surface(np.full_like(yy2, x+dx), yy2, zz2)

    xx2, zz2= np.meshgrid(xx, zz)
    ax.plot_surface(xx2, np.full_like(yy2, y), zz2)
    ax.plot_surface(xx2, np.full_like(yy2, y+dy), zz2)
    
def compute_z(A,B,C,D,x,y):
    value = -(A*x + B*y + D)/C 
    return value

def plane(point1, point2, point3):
    x1 = 0
    y1 = point1[1]
    z1 = point1[2]
    x2 = point2[0]
    y2 = point2[1]
    z2 = point2[2]
    x3 = point3[0]
    y3 = point3[1]
    z3 = point3[2]
    A = (y3 - y1)*(z3 - z1) - (z2 -z1)*(y3 - y1)
    B = (x3 - x1)*(z2 - z1) - (x2 - x1)*(z3 - z1)
    C = (x2 - x1)*(y3 - y1) - (x3 - x1)*(y2 - y1)
    D = -(A * x1 + B * y1 + C * z1)
    return A,B,C,D

def iscollison(s_point, e_point, check_arr, equivalent_size, range_of_rep): 
    for j in range(len(check_arr)):
        # collision check #
        ob_point = check_arr[j]
        #计算用到的向量
        ab=e_point-s_point
        ap=ob_point-s_point
        bp=ob_point-e_point
        #计算投影长度，并做正则化处理
        r = np.dot(ap,ab)/(np.linalg.norm(ab))**2
        #分了三种情况
        if r > 0 and r < 1:
            dis = sqrt((np.linalg.norm(ap))**2 - (r * np.linalg.norm(ab))**2)
        elif r >= 1:
            dis = np.linalg.norm(bp)
        else:
            dis = np.linalg.norm(ap)        
        if dis<equivalent_size+range_of_rep/2.0:
            return True
    return False

def APF_TS_3D():
    # 备注：TS搜索时，强度设置的合理性很重要，吸引力过大,障碍物边缘的斥力占比小，可能导致TS出现回头现象，TS搜索提前结束；吸引力过大导致靠近障碍物  参数问题好像很明显的样子
    # 目前TS搜索方向，极小点与goal连线区分两边进行搜索

    # *************** 起止点 *************** # 
    start_position = np.array([0.39, 0.29, 0.22], dtype=np.float64) # 起点位置左下，分别代表x,y,vx,vy
    target_positon = np.array([0.45, -0.29, 0.33], dtype=np.float64) # 目标位置

    # 障碍物位置
    obstacle_arr = np.array([
        [0.42,0,0.28],
        [0.35,-0.1,0.23]])

    # obstacle_arr = np.array([
    #     [0.7, 1, 0, 0],
    #     [1, 0.7, 0, 0]])

    P   = np.vstack((target_positon,obstacle_arr))  # 将目标位置和障碍物位置合放在一起
    num = P.shape[0]                                # 障碍与目标总计个数,堆叠起来只用于数组的初始化

    # *************** APF参数 *************** # 
    Eta_att         = 1    # 引力的增益系数
    Eta_rep_ob      = 1   # 斥力的增益系数
    equivalent_size = 0.09    # 需要考虑机器人实际大小与障碍物大小，目前考虑2D圆
    range_of_rep    = 0.09  # obstacle斥力范围，障碍物外圈范围
    len_step        = 0.02 # 步长
    n               = 1    # AAPF              
    Num_iter        = 1000 # 最大循环迭代次数

    path  = []                     # 保存车走过的每个点的坐标
    path_APF = []                  # 分段保存APF路径
    temp_path = []
    ori   = np.zeros((num,3))      # 保存车辆当前位置与障碍物的方向向量，方向指向车辆；以及保存车辆当前位置与目标点的方向向量，方向指向目标点
    unit_ori = np.zeros((num,3))   #  保存车辆当前位置与障碍物的单位方向向量，方向指向车辆；以及保存车辆当前位置与目标点的单位方向向量，方向指向目标点
    F_rep_ob = np.zeros((len(obstacle_arr),3))  # 存储每一个障碍到车辆的斥力,带方向
    # v=np.linalg.norm(start_position[2:4]) # 设车辆速度为常值

    # ************ 极小值检查程序TS相关初始化 ******************** #
    fall_min_thresd = len_step
    tb_len = randint(1,3)  # 5-11之间的随机数,禁忌表长度
    nerb_count = 10         # 邻域解个数  
    TS_max_iter = 150       # 最大迭代次数
    w          = 0.7        # 自适应权重
    tabu       = []         # 禁忌表
    first_time_flag = True  # 第一次进入TS确认第一步探索方向
    TS_run_step = 0         # 记录运行次数，先考虑作为跳出极小值的判断条件吧
    path_TS = []
    len_step_TS = 2*len_step
    thetas = []        # 采样点
    min_point = []        # 采样点
    TS_step_record = []
    sample_points = []        # 采样点做图用

    # 其他
    A1 = 1
    B1 = 1
    C1 = 1
    D1 = 1
    ## ******************初始化结束，开始主体循环********************** # 
    current_position = copy.deepcopy(start_position)  # 当前车辆位置
    count = 0
    APF_stauts = True     # 程序两个状态，True代表APF，False代表TS
    start = time.time()

    for i in range(Num_iter):
        path.append(copy.deepcopy(current_position))
        temp_path.append(copy.deepcopy(current_position))
        # 到达终点判断，均方差
        if  np.linalg.norm(current_position-target_positon) < 0.03:
            break
        # 判断是否陷入极小值,True则进入TS程序
        if len(path)>5:
            if np.linalg.norm(path[-1]-path[-5])<fall_min_thresd and APF_stauts:
                APF_stauts = False
                path_APF.append(copy.deepcopy(temp_path))
                temp_path.clear()
                temp_path.append(copy.deepcopy(current_position))

    # ************************* APF START ********************************#
        if APF_stauts:
            dists = []       # 保存车辆当前位置与障碍物的距离以及车辆当前位置与目标点的距离
            # 计算车辆当前位置与障碍物的距离和单位方向向量
            for j in range(len(obstacle_arr)):
                ori[j] = current_position - obstacle_arr[j]
                dists.append(np.linalg.norm(ori[j])-equivalent_size)
                unit_ori[j] = ori[j]/dists[j]

            # 计算车辆当前位置与目标的距离和单位方向向量
            ori[-1] = target_positon - current_position
            dists.append(np.linalg.norm(ori[-1]))
            unit_ori[-1] = ori[-1]/dists[-1]

            # *************** 计算引力 *************** # 
            # if(dists[-1]>10):
            #     dists[-1]=10
            F_att = Eta_att*dists[-1]*unit_ori[-1]  

            # *************** 计算斥力 *************** # 
            # 在原斥力势场函数增加目标调节因子（即车辆至目标距离），以使车辆到达目标点后斥力也为0
            # AAPF，所以两个斥力分量
            for j in range(len(obstacle_arr)):
                if dists[j] >= range_of_rep:
                    F_rep_ob[j] = np.array([0, 0, 0])
                else:
                    # 障碍物的斥力1，方向由障碍物指向车辆
                    F_rep_ob1_abs = Eta_rep_ob * \
                        (1 / dists[j] - 1 / range_of_rep) * \
                        (pow(dists[-1],5)/(1+pow(dists[-1],5))) / dists[j] ** 2  # 斥力大小
                    F_rep_ob1 = F_rep_ob1_abs*unit_ori[j]  # 斥力向量
                    # 障碍物的斥力2，方向由车辆指向目标点
                    F_rep_ob2_abs = n/2 * Eta_rep_ob * \
                        (1 / dists[j] - 1 / range_of_rep) ** 2 * \
                        pow(1/(1+pow(dists[-1],5)),2)* \
                            pow(dists[-1],4)# 斥力大小
                    F_rep_ob2 = F_rep_ob2_abs * unit_ori[-1]  # 斥力向量
                    # 改进后的障碍物合斥力计算
                    F_rep_ob[j] = F_rep_ob1 + F_rep_ob2

            # *************** 计算合力和方向 *************** # 
            # print("F_ATT     :  ",F_att)
            # print("F_REP     :  ",np.sum(F_rep_ob, axis=0))
            F_sum = F_att+np.sum(F_rep_ob, axis=0)      # np.sum 可以把矩阵的对应行相加，axis记得写

            UnitVec_Fsum = 1 / np.linalg.norm(F_sum) * F_sum
            # *************** 位置更新，计算车的下一步位置 *************** # 
            current_position = copy.deepcopy(current_position + len_step * UnitVec_Fsum)
    # **************************** APF END ************************************#

    # ************************* 极小值检查程序TS ****************************** #
        if not APF_stauts:
            
            #  # ************** 产生邻域解 *********** #
            x_nears = []
            if first_time_flag:
                # 手动给方向好了
                A1, B1, C1, D1 = plane(target_positon, current_position, target_positon)
                tempori = target_positon[0:2] - current_position[0:2]
                search_bound = atan2(tempori[1],tempori[0])      # sd:当前点与的目标连线区分左右，而不是current
                thetas = np.linspace(search_bound,search_bound-pi,nerb_count)
                first_time_flag = False
                print("ts")

            for theta in thetas:       
                x = current_position[0]+cos(theta)*len_step_TS
                y = current_position[1]+sin(theta)*len_step_TS
                z = compute_z(A1,B1,C1,D1,x,y)
                x_nears.append([x,y,z])
            sample_points.append(copy.deepcopy(x_nears))
            # print(sample_points[0])
            # ************ 生成各邻域点后，计算邻域函数值 ************#
            fitvalues = []                                # 保存邻域函数值
            ori_TS    = np.zeros((num,3))                 # 邻域点与障碍物的方向向量
            for key in x_nears:
                dists_TS  = []                                # 计算这些领域点与障碍物的距离
                # 邻域点与障碍物
                for j in range(len(obstacle_arr)):
                    ori_TS[j] = key - obstacle_arr[j]  # 先重新计算距离
                    dists_TS.append(np.linalg.norm(ori_TS[j])-equivalent_size)
                # 邻域点与目标点
                ori_TS[-1] = target_positon - key
                dists_TS.append(np.linalg.norm(ori_TS[-1]))

                ## 计算斥力强度
                intens_sum = 0
                for m in range(len(obstacle_arr)):
                    if dists_TS[m] <= 0.01:
                        intens_sum = intens_sum+ 40
                    elif dists_TS[m] >= range_of_rep:
                        pass
                    else :
                        rep = 0.5*Eta_rep_ob * pow((1 / dists_TS[m] - 1 / range_of_rep),2) * (pow(dists_TS[-1],5)/(1+pow(dists_TS[-1],5)))
                        intens_sum = rep+intens_sum
                # print("斥力：    ",intens_sum)
                ## 计算引力强度
                intens_sum = intens_sum+ 0.5*Eta_att*(dists_TS[-1]**2)
                fitvalues.append(intens_sum)
            # print(fitvalues)
            # ************ 确认方向，禁忌判断 ************#
            index_arr = np.argsort(fitvalues)                # 升序排列
            index_effective = 0
            ban_flag = False                     # 判断是否在禁忌表内
            for index in index_arr:
                for j in range(len(tabu)):
                    if np.linalg.norm(np.array(x_nears[index])-np.array(tabu[j]))<len_step:
                        ban_flag = True
                        break
                if ban_flag:
                    ban_flag = False
                else:
                    index_effective = index
                    ban_flag = False
                    break
            
            # TS搜索势场强度低的方向前进
            if not ban_flag:
                tabu.append(current_position)              # 更新禁忌表
                if len(tabu)>tb_len:
                    tabu.pop(0)
                current_position = np.array(x_nears[index_effective])      # 更新状态
                # print(index_effective)
                # TS搜索结束判定1: 搜索方向以及转向目标点，认为跳出极小
                if index_effective <= 2  :      
                    APF_stauts = True
                    first_time_flag = True
                    TS_run_step = 0
                    tabu.clear()
                    temp_path.append(copy.deepcopy(current_position))
                    path_TS.append(copy.deepcopy(temp_path))
                    temp_path.clear()
                    # TS_step_record.append(len(path))
                    
            # TS搜索结束判定2：达到迭代次数
            TS_run_step = TS_run_step+1
            if TS_run_step == TS_max_iter:
                APF_stauts = True
                first_time_flag = True
                TS_run_step = 0                    # 恢复一些重复利用的全局变量
                tabu.clear()
                temp_path.append(copy.deepcopy(current_position))
                path_TS.append(copy.deepcopy(temp_path))
                temp_path.clear()
                # TS_step_record.append(len(path_TS))
        count = count+1   
    # ************************* 极小值检查程序TS end ****************************** #

    print("run step:     ",count)
    end = time.time()
    print('耗时：{}秒'.format(end - start))

    # ************************* 额外处理 ****************************** #
    path.append(target_positon) # 最后把目标点也添加进路径中
    rigion = np.asarray(path)
    temp_path.append(target_positon)
    path_APF.append(copy.deepcopy(temp_path))
    # print(path[0],path[-1],path[15],path[35])
    # ************************* 剪枝程序 START ****************************** #
    unidirectionalPath = [path[0]]
    pointTem=np.asarray([path[0][0],path[0][1],0])  
    for i in range(3,len(path)):
        pointTem_end = path[i]
        if iscollison(pointTem,pointTem_end,obstacle_arr,equivalent_size,range_of_rep):
            pointTem = path[i-1]
            unidirectionalPath.append(path[i-1])
    unidirectionalPath.append(path[-1])
    unidirectionalPath = np.array(unidirectionalPath) # 转为numpy
    # print(path.shape)
    NHEBNode = elastic_bands_3D.EBNode(maxExpension = 0.02, minExpension = 0.02, k_inter = 0.1, k_extel = 5, 
                    circle = 30, path_arr=path,obs_arr=obstacle_arr, ob_radius=equivalent_size, pause_sec = 0.001)
    path, dis = NHEBNode.run()
    
    fig, ax = plt.subplots()
    min_index=np.argmin(dis)
    pathi= np.asarray(dis)
    t = np.linspace(0,0.033*len(dis),len(dis))
    t = np.asarray(t)
    plt.plot(t,pathi, ls = '-',color='k',LineWidth=2)  # 路径点
    temp =("%.3f"%pathi[min_index])
    tempt =("%.1f"%t[min_index])
    show_min='['+str(tempt)+','+str(temp)+']'
    plt.annotate(show_min,xy=(t[min_index],pathi[min_index]),xytext=(t[min_index],pathi[min_index]+0.01))
    plt.plot(t[min_index],pathi[min_index],marker = 'o', ms = 5, mec = 'k',mfc = 'k')
    plt.ylabel("Distance(m)",fontsize = 10)
    plt.xlabel("Time(s)",fontsize = 10)
    plt.legend()
    plt.savefig("2.svg", dpi=300,format="svg")
    print(len(path),path[0],path[80],path[160],path[-1])
    path = np.array(path[0:0])
    # print(unidirectionalPath)
    vt.node(path, unidirectionalPath, rigion)
    
    # # ************************** 绘制路径 **************************** # 
    # path = np.array(path) # 转为numpy
    # print(path[-2])

    # fig = plt.figure()
    # ax = fig.gca(projection='3d')

    # # # 画障碍球
    # for i in range(len(obstacle_arr)):                     #障碍物(圆)
    #     u = np.linspace(0, 2 * np.pi, 100) 
    #     v = np.linspace(0, np.pi, 100) 
    #     x = np.outer(np.cos(u), np.sin(v)) +obstacle_arr[i,0]
    #     y = np.outer(np.sin(u), np.sin(v)) +obstacle_arr[i,1]
    #     z = np.outer(np.ones(np.size(u)), np.cos(v)) +obstacle_arr[i,2]
    #     ax.plot_surface(x, y, z)
        
    # ax.scatter(start_position[0], start_position[1], start_position[2], marker='o')# 绘制起点
    # ax.scatter(target_positon[0], target_positon[1], target_positon[2], marker='^')# 绘制终点

    # ax.plot(path[0:(len(path)-1), 0], path[0:(len(path)-1), 1], path[0:(len(path)-1),2], color = "k", label='parametric curve')

    # # ************************** 绘制TS路径 **************************** # 
    # for i in range(len(path_TS)):
    #     draw_nd=np.asarray(path_TS[i]) # 转为numpy
    #     plt.plot(draw_nd[:,0],draw_nd[:,1], draw_nd[:,2], 'r')  # 路径点
        
    # ax.plot(unidirectionalPath[:, 0], unidirectionalPath[:, 1], unidirectionalPath[:,2], color = "b", label='parametric curve')

    # # 显示图形
    # plt.pause(10000) 
APF_TS_3D()

